#include "imu_math.h"

/**
 * @brief 将imu加速度计和陀螺仪数据转为四元数
 * @param gyro 陀螺仪三维指针
 * @param accel 加速度计三维指针
 * @param quater 返回的四元数指针
 * @param freq 采样频率
 **/
void imu_math_raw_to_quaternion(imu_vector3f *gyro,imu_vector3f *accel,double freq,imu_vector4f* quater)
{
  double halfT=(1/(freq*2));
  double norm;
  double vx, vy, vz;

  /*PID-PI*/
  const double ki=0.005f,kp=1.50f;
  double ex, ey, ez;
  static double exInt,eyInt,ezInt;

  // 加速度计测量的重力向量
  norm = inv_sqrt(sq(accel->x) + sq(accel->y)+ sq(accel->z));
  accel->x *= norm;
  accel->y *= norm;
  accel->z *= norm;

  // 陀螺仪估计的重力向量
  vx = 2.0 * (quater->q1 * quater->q3 - quater->q0 * quater->q2);
  vy = 2.0 * (quater->q0 * quater->q1 + quater->q2 * quater->q3);
  vz = sq(quater->q0) - sq(quater->q1) - sq(quater->q2) + sq(quater->q3);

  // 计算加速度和陀螺仪的误差
  ex = (accel->y * vz - accel->z * vy);
  ey = (accel->z * vx - accel->x * vz);
  ez = (accel->x * vy - accel->y * vx);

  // 对误差进行积分
  exInt = exInt + ex * ki;
  eyInt = eyInt + ey * ki;
  ezInt = ezInt + ez * ki;

  // PID求和
  gyro->x = gyro->x + kp * ex + exInt;
  gyro->y = gyro->y + kp * ey + eyInt;
  gyro->z = gyro->z + kp * ez + ezInt;

  // 更新四元数
  quater->q0 = quater->q0 + (-quater->q1 * gyro->x - quater->q2 * gyro->y - quater->q3 * gyro->z) * halfT;
  quater->q1 = quater->q1 + (quater->q0 * gyro->x + quater->q2 * gyro->z - quater->q3 * gyro->y) * halfT;
  quater->q2 = quater->q2 + (quater->q0 * gyro->y - quater->q1 * gyro->z + quater->q3 * gyro->x) * halfT;
  quater->q3 = quater->q3 + (quater->q0 * gyro->z + quater->q1 * gyro->y - quater->q2 * gyro->x) * halfT;

  // 单位化四元数 
  norm = inv_sqrt(sq(quater->q0) + sq(quater->q1) + sq(quater->q2) + sq(quater->q3));
  quater->q0*=norm;
  quater->q1*=norm;
  quater->q2*=norm;
  quater->q3*=norm;

}

/**
 * @brief 将四元数转为欧拉角
 * @param quater 四元数指针
 * @param euler 返回的欧拉角指针
 **/
void quaternion_to_euler(imu_vector4f* quater, imu_euler* euler) 
{
  if(quater->q0==0.0||quater->q1==0.0||quater->q2==0.0||quater->q3==0.0)return;
  imu_euler euler_d;
  float sq0 = sq(quater->q0);
  float sq1 = sq(quater->q1);
  float sq2 = sq(quater->q2);
  float sq3 = sq(quater->q3);

  euler_d.yaw = atan2(2.0 * (quater->q1 * quater->q2 + quater->q3 * quater->q0), (sq1 - sq2 - sq3 + sq0));
  euler_d.pitch = asin(-2.0 * (quater->q1 * quater->q3 - quater->q2 * quater->q0) / (sq1 + sq2 + sq3 + sq0));
  euler_d.roll = atan2(2.0 * (quater->q2 * quater->q3 + quater->q1 * quater->q0), (-sq1 - sq2 + sq3 + sq0));

  euler->yaw = euler_d.yaw*RAD_TO_DEG;
  euler->pitch = euler_d.pitch*RAD_TO_DEG;
  euler->roll = euler_d.roll*RAD_TO_DEG;
}

/**
 * @brief 角度差值计算
 * @param target 目标值,支持任意角度
 * @param real 实际值,支持任意角度
 * @return 从real到达target所需值
 * @note 目标(1.距离最短（距离不能超过180） 2.R在经过“结果个角度”能到达T（1.符号代表方向 2.存在基准方向）)
 **/
float imu_math_cal_euler_error(const float target, float real)
{
  float error0 = 0.0f, error1 = 0.0f, error_return = 0.0f;

  float _target = fmodf(target, 360.00000f), _real = fmodf(real, 360.00000f);

  /*计算不跨越特异点的R到T的距离 |R-T|=error0,error0<=360*/
  error0 = fabs(_real - _target);

  /*计算跨越特异点的R到T的距离，这种计算方法会使这个结果的基准方向与原定的基准方向相反
  (1.error1(R->T)加error0(T->R)必然等于360；2.向量加法) 360-error0=error1*/
  error1 = 360 - error0;

  /*返回最小的距离*/
  /*我们以不跨越特异点的方向为基准，所以给error1取反*/
  error_return = fabs(error0) < fabs(error1) ? error0 : -error1;

  /*设置方向，在以不跨越特异点的方向为基准的前提下，负应为逆时针，正应为顺时针*/
  error_return = _real >= _target ? -error_return : error_return;

  return error_return;
}

/**
 * @brief 向量减法
 * @param a 减数
 * @param b 被减数
 * @return 结果
 **/
imu_vector3f vector_sub(imu_vector3f a,imu_vector3f b)
{
  imu_vector3f c;

  c.x=a.x-b.x;
  c.y=a.y-b.y;
  c.z=a.z-b.z;

  return c;
}

/**
 * @brief 反平方根
 * @param x 输入
 * @return 反平方根结果
 **/
float inv_sqrt(float x)
{
  float halfx = 0.5f * x;
  float y = x;
  long i = *(long*)&y;

  i = 0x5f3759df - (i>>1);
  y = *(float*)&i;
  y = y * (1.5f - (halfx * y * y));

  return y;
}

/**
 * @brief 平方
 * @param v
 * @return 平方结果
 **/
float sq(float v)
{
  return v*v;
}